﻿#include "SlamWebSocket.h"
#include <QFile>
#include <QNetworkAccessManager>
#include <QNetworkReply>
#include <QNetworkRequest>
#include <QSharedMemory>
#include <QTimer>
#include <QVariantList>
#include <QVariantMap>
#include "JsonUtil.h"
// #include <QSslSocket>

#define ACTION_START_LASER ("startLaser")
#define ACTION_STOP_LASER ("stopLaser")
#define ACTION_GET_CLOUD ("getCloud")
#define ACTION_LS_DATA ("lsdata")
#define ACTION_GET_ROBPOS ("getRobotPos")

// change0425
// #define ACTION_RESET_LASER  ("resetLaser")

#define WEBSOCK_IP (QString("http://192.168.1.188:"))
#define WEBSOCK_PORT (QString("29876/"))
#define WEBSOCK_IP_PORT (WEBSOCK_IP + WEBSOCK_PORT)

#define INTERVAL_GET_PLATFORM_STATE (1000)

SlamWebSocket::SlamWebSocket(int argc, char** argv, QObject* parent)
    : argc_(argc), argv_(argv), QObject(parent)
{
    m_netAccesser = new QNetworkAccessManager(this);
    connect(m_netAccesser, SIGNAL(finished(QNetworkReply*)), this,
            SLOT(onReplyFinished(QNetworkReply*)));

    m_timerStartLaser = new QTimer(this);
    m_timerStartLaser->setSingleShot(true);
    connect(m_timerStartLaser, &QTimer::timeout, this,
            &SlamWebSocket::onTimerStartLaser);

    m_smSLAM = new QSharedMemory("sm_slam");

    //    QFile f("laser.pts");
    //    f.open(QIODevice::ReadOnly);
    //    QByteArray baPt = f.readAll();
    //    float pts[12] = {0.0};
    //    memcpy(pts, baPt.data(), 12*4);
    //    for (int i=0; i<12; ++i)
    //    {
    //        qDebug() << i << ": " << pts[i];
    //    }

    m_bIsBusy = false;
    m_timerGetPlatformState = new QTimer(this);
    m_timerGetPlatformState->setInterval(INTERVAL_GET_PLATFORM_STATE);
    m_timerGetPlatformState->setSingleShot(false);
    connect(m_timerGetPlatformState, &QTimer::timeout, this,
            &SlamWebSocket::onGetPlatformState);
    m_timerGetPlatformState->start();
    qDebug() << "SlamWebSocketRun";

    m_ros_spin_thread =
        std::thread{std::bind(&SlamWebSocket::rosSpinThread, this)};
}

SlamWebSocket::~SlamWebSocket()
{
    rclcpp::shutdown();
}

void SlamWebSocket::callStartLaser(const QString& serial)
{
    QMetaObject::invokeMethod(this, "startLaser", Qt::QueuedConnection,
                              Q_ARG(QString, serial));
}

void SlamWebSocket::callStopLaser()
{
    QMetaObject::invokeMethod(this, "stopLaser", Qt::QueuedConnection);
}

void SlamWebSocket::callGetCloud()
{
    QMetaObject::invokeMethod(this, "getCloud", Qt::QueuedConnection);
}

void SlamWebSocket::rosSpinThread()
{
    rclcpp::init(argc_, argv_);
    m_robotCmdNode = std::make_shared<RobotCmdNode>(this);
    rclcpp::spin(m_robotCmdNode);
    rclcpp::shutdown();
}

void SlamWebSocket::startLaser(const QString& serial)
{
    m_strSerial = serial;
    m_strCurAction = ACTION_START_LASER;
    QNetworkRequest req;
    QString strUrl = WEBSOCK_IP_PORT + "StartAcquisition";
    req.setUrl(QUrl(strUrl));
    req.setRawHeader("Content-Type", "application/json");
    QVariantMap mapId;
    mapId.insert("robotId", "09b2bb5a8e4b4022be40d8d27e75c7a9");
    QByteArray ba;
    JsonUtil::varMap2ByteArray(mapId, ba);
    m_netAccesser->post(req, ba);
    qDebug() << "startLaserRun";
}

void SlamWebSocket::stopLaser()
{
    m_strCurAction = ACTION_STOP_LASER;
    QNetworkRequest req;
    QString strUrl = WEBSOCK_IP_PORT + "FinishAcquisition";
    req.setUrl(QUrl(strUrl));
    req.setRawHeader("Content-Type", "application/json");
    QVariantMap mapId;
    mapId.insert("robotId", "09b2bb5a8e4b4022be40d8d27e75c7a9");
    QByteArray ba;
    JsonUtil::varMap2ByteArray(mapId, ba);
    m_netAccesser->post(req, ba);
    qDebug() << "stopLaserRun";
}

void SlamWebSocket::getCloud()
{
    m_strCurAction = ACTION_GET_CLOUD;
    QNetworkRequest req;
    QString strUrl =
        WEBSOCK_IP_PORT + "PointCloud?robotId=09b2bb5a8e4b4022be40d8d27e75c7a9";
    req.setUrl(QUrl(strUrl));
    m_netAccesser->get(req);
    qDebug() << "getCloudRun";
}

void SlamWebSocket::getRobotPos()
{
    m_strCurAction = ACTION_GET_ROBPOS;
    QNetworkRequest req;
    QString strUrl =
        WEBSOCK_IP_PORT + "RobotPose?robotId=09b2bb5a8e4b4022be40d8d27e75c7a9";
    req.setUrl(QUrl(strUrl));
    m_netAccesser->get(req);
}

void SlamWebSocket::testData(const QString& serial)
{
    if (!m_smSLAM->isAttached()) {
        m_smSLAM->attach();
    }

    m_strSerial = serial;
    QFile f("laser.pts");
    f.open(QIODevice::ReadOnly);
    QByteArray ba = f.readAll();
    QList<float> listX, listY, listZ;

    float xmin = 999999.0;
    float ymin = 999999.0;
    float zmin = 999999.0;
    float xmax = -999999.0;
    float ymax = -999999.0;
    float zmax = -999999.0;
    for (int i = 0; i < ba.size() / 12; ++i) {
        float x, y, z;
        memcpy(&x, ba.data() + i * 4 + 4 * 0, 4);
        memcpy(&y, ba.data() + i * 4 + 4 * 1, 4);
        memcpy(&z, ba.data() + i * 4 + 4 * 2, 4);
        xmin = xmin > x ? x : xmin;
        xmax = xmax < x ? x : xmax;
        ymin = ymin > y ? y : ymin;
        ymax = ymax < y ? y : ymax;
        zmin = zmin > z ? z : zmin;
        zmax = zmax < z ? z : zmax;
    }

    // sigLaserDataReady(m_strSerial, limits, ba);

    QVariantMap mapJson;
    mapJson.insert("action", "lsdata");
    mapJson.insert("serial", serial);
    mapJson.insert("color", "none");
    mapJson.insert("xmin", xmin);
    mapJson.insert("xmax", xmax);
    mapJson.insert("ymin", ymin);
    mapJson.insert("ymax", ymax);
    mapJson.insert("zmin", zmin);
    mapJson.insert("zmax", zmax);
    mapJson.insert("points", ba.size() / 12);

    QByteArray baSLAM = JsonUtil::packDataFE(mapJson, ba);  // ef->fe

    int nBaSLAMDataSize = baSLAM.size();
    // baCloud.size();
    m_smSLAM->lock();
    memcpy((char*)m_smSLAM->data(), &nBaSLAMDataSize, 4);
    memcpy((char*)m_smSLAM->data() + 4, baSLAM.data(), nBaSLAMDataSize);
    m_smSLAM->unlock();

    m_robotCmdNode->pubSlamReady();
}

void SlamWebSocket::onReplyFinished(QNetworkReply* reply)
{

    // qDebug() <<
    // "-----------------------------------------onReplyFinished-------------------------------------";
    // qDebug() << "curAction: " << m_strCurAction;
    QByteArray bytes = reply->readAll();

    if (ACTION_LS_DATA == m_strCurAction) {
        //        sigLaserDataReady(m_strSerial, bytes);
        qDebug() << "start to recv las data";
        QFile f("laser.pts");
        f.open(QIODevice::WriteOnly | QIODevice::Truncate);
        f.write(bytes);

        testData(m_strSerial);

        m_bIsBusy = false;
    } else {
        QVariantMap mapBa;
        if (JsonUtil::byteArray2VarMap(bytes, mapBa)) {
            QString strBytes = QString(bytes);
            sigReply(strBytes);

            int nCode = mapBa.value("code").toInt();
            // qDebug() << "cur action: " << m_strCurAction << "  code:" <<
            // nCode;

            // qDebug() << "mapBa: " << mapBa;

            if (200 == nCode) {
                //                if (ACTION_STOP_LASER == m_strCurAction)
                //                {
                //                    m_strCurAction = ACTION_GET_CLOUD;
                //                    m_timerStartLaser->start(100);
                //                    goto DEL_REPLY;
                //                }

                QString strBinPath = mapBa.value("data", QVariantMap())
                                         .toMap()
                                         .value("binPath", "")
                                         .toString();

                if (!strBinPath.isEmpty())
                {
                    qDebug() << "--------------------------------------------------"
                                "----------strBinPaht: "
                            << strBinPath;
                }
                if (!strBinPath.isEmpty()) {
                    m_bIsBusy = true;
                    QVariantMap mapData =
                        mapBa.value("data", QVariantMap()).toMap();
                    m_strCldMd5 = mapData.value("md5").toString();
                    m_nCldSize = mapData.value("length").toInt();
                    m_strCurAction = ACTION_LS_DATA;
                    QNetworkRequest req;
                    QString strUrl = WEBSOCK_IP_PORT + strBinPath.mid(1);
                    req.setUrl(QUrl(strUrl));
                    m_netAccesser->get(req);
                    goto DEL_REPLY;
                }

                // send platform state
                //                QString strEulerAngles = mapBa.value("data",
                //                QVariantMap()).toMap().value("eulerAngles",
                //                "").toString();

                if (mapBa.value("data", QVariantMap())
                        .toMap()
                        .contains("eulerAngles")) {
                    QVariantMap mapPlatformState = mapBa.value("data").toMap();
                    // qDebug() << "SLAM POS: " << mapPlatformState;
                    // sigPlatformStateReady(mapPlatformState);

                    QVariantMap mapPos =
                        mapPlatformState.value("position").toMap();
                    QVariantMap mapEul =
                        mapPlatformState.value("eulerAngles").toMap();

                    m_robotCmdNode->pubSlamPos(mapPos.value("x").toFloat(),
                                               mapPos.value("y").toFloat(),
                                               mapPos.value("z").toFloat(),
                                               mapEul.value("roll").toFloat(),
                                               mapEul.value("pitch").toFloat(),
                                               mapEul.value("yaw").toFloat());
                }
            }
        }
    }

    //    else
    //    {
    //        QFile f("laser.pts");
    //        f.open(QIODevice::WriteOnly);
    //        f.write(bytes);
    //    }

    //    {"code":200,"message":"成功","generalMessage":"SUCCESS","data":{"binPath":"/resource/point-cloud/output"}}

DEL_REPLY:

    reply->deleteLater();
}

void SlamWebSocket::onTimerStartLaser()
{
    if (ACTION_STOP_LASER == m_strCurAction) {
        stopLaser();
    } else if (ACTION_GET_CLOUD == m_strCurAction) {
        getCloud();
    }
    qDebug() << "onTimerStartLaserRun";
}

void SlamWebSocket::onGetPlatformState()
{
    if (!m_bIsBusy) {
        //        qDebug() << "before getPlatformState";
        getRobotPos();
    }
}
